package org.kealinghornets.nxtdroid;

import org.kealinghornets.nxtdroid.NXT.LightSensor;
import org.kealinghornets.nxtdroid.NXT.NXTThread;
import org.kealinghornets.nxtdroid.NXT.TouchSensor;

/**
 * Created by jychuah on 11/3/13.
 */
public class MyNXTThread extends NXTThread {

    int state = 1;
    LightSensor eyeball;

    public MyNXTThread() {
        setNXTThreadName("Koolaid");
    }

    public void run() {
        while (state != -1) {
            switch(state) {
                case 1 : do_state_1(); break;
                case 2 : do_state_2(); break;
                case 3 : do_state_3(); break;
                case 4 : do_state_4(); break;
                case 5 : do_state_5(); break;
                case 6 : do_state_6(); break;
                case 7 : do_state_7(); break;
                //case 8 : do_state_8(); break;
                //case 9 : do_state_9(); break;
                //case 10 : do_state_10(); break;
                //case 11 : do_state_11(); break;
                //case 12 : do_state_12(); break;
                //case 13 : do_state_13(); break;
                //case 14 : do_state_14(); break;
                default : state = -1; break;
            }
        }
    }

    //Void = 0-300, Table = 350-475, White = 480-1000

    public void do_state_1() {
        log("I'm in state 1. Turning left and then reading the light value.");

        runMotor(PORT_B, 85);
        wait(380);
        stopMotor(PORT_B);

        wait(3000);

        eyeball = getActiveLightSensor(PORT_1);

        if (eyeball.getLightValue() > 0 && eyeball.getLightValue() < 300) {
            log("I'm over the void");
            state = 2;
        }

        if (eyeball.getLightValue() > 350 && eyeball.getLightValue() < 475) {
            log("I'm over the table");
            state = 3;
        }

        if (eyeball.getLightValue() > 480 && eyeball.getLightValue() < 1000) {
            log("The White! The White!");
        }
    }

    public void do_state_2() {
        log ("I'm in state 2. Backing up to the middle of table 1 and turning right.");

        runMotor(PORT_A, -85); runMotor(PORT_B, -80);
        wait(350);
        stopMotor(PORT_A); stopMotor(PORT_B);

        wait(3000);

        runMotor(PORT_A, 85);
        wait(380);
        stopMotor(PORT_A);

        wait(3000);

        state = 4;
    }

    public void do_state_3() {
        log ("I'm in state 3. Moving forward to the middle of table 1.");

        runMotor(PORT_A, 85); runMotor(PORT_B, 80);
        wait(350);
        stopMotor(PORT_A); stopMotor(PORT_B);

        wait(3000);

        state = 4;
    }

    public void do_state_4() {
        log("I'm in state 4. Turning right and then reading the light value.");

        runMotor(PORT_A, 85);
        wait(400);
        stopMotor(PORT_A);

        wait(3000);

        if (eyeball.getLightValue() > 0 && eyeball.getLightValue() < 300) {
            log("I'm over the void");
            state = 5;
        }

        if (eyeball.getLightValue() > 375 && eyeball.getLightValue() < 475) {
            state = 6;
        }
    }

    public void do_state_5() {
        log ("I'm in state 5. Full turn and then moving forward to the exact center of table 1, then turning right.");

        runMotor(PORT_B, 85);
        wait(825);
        stopMotor(PORT_B);

        wait(3000);

        runMotor(PORT_A, 85);runMotor(PORT_B, 85);
        wait(700);
        stopMotor(PORT_A); stopMotor(PORT_B);

        wait(3000);

        runMotor(PORT_A, 85);
        wait(450);
        stopMotor(PORT_A);

        wait(3000);

        state = 7;
    }

    public void do_state_6() {
        log ("I'm in state 6. Moving forward to the exact center of table 1, then turning right.");

        state = 7;
    }

    public void do_state_7() {
        log ("I'm in state 7. Searching for the bridge.");

        runMotor(PORT_A, 85); runMotor(PORT_B, 80);
        wait(50);
        stopMotor(PORT_A); stopMotor(PORT_B);

        if (eyeball.getLightValue() > 0 && eyeball.getLightValue() < 300) {
            state = 9;
        }
        if (eyeball.getLightValue() > 350 && eyeball.getLightValue() < 475) {
            state = 7;
        }

        if (eyeball.getLightValue() > 480 && eyeball.getLightValue() < 1000) {
            state = 8;
        }
    }

    public void do_state_8() {
        log ("I'm in state 8. Moving forward to the exact center of table 2 and then turning right.");

        runMotor(PORT_A, 85); runMotor(PORT_B, 85);
        wait(2000);
        stopMotor(PORT_A); stopMotor(PORT_B);

        runMotor(PORT_A, 85);
        wait(400);
        stopMotor(PORT_A);
    }

    public void do_state_9() {
        log("I'm in state 9. Full turn and then moving forward to the exact center of table 2.");

        runMotor(PORT_A, -85); runMotor(PORT_B, -85);
        wait(2000);
        stopMotor(PORT_A); stopMotor(PORT_B);

        state = 10;
    }

    public void do_state_10() {
        log ("I'm in state 10. Turning left.");

        runMotor(PORT_B, 85);
        wait(825);
        stopMotor(PORT_B);

        state = 13;
    }

    public void do_state_11() {
        log ("I'm in state 11. Full turn.");

        runMotor(PORT_A, 85); runMotor(PORT_B, -85);
        wait(450);
        stopMotor(PORT_A); stopMotor(PORT_B);

        state = 14;
    }

    public void do_state_12() {
        log ("I'm in state 12. Dropping the ball.");

        runMotor(PORT_C, 50);
        wait(1000);
        stopMotor(PORT_C);

        state = -1;
    }

    public void do_state_13() {
         log ("I'm in state 13. Searching for the target.");

         runMotor(PORT_A, 85); runMotor(PORT_B, 85);
         wait(50);
         stopMotor(PORT_A); stopMotor(PORT_B);

         eyeball = getActiveLightSensor(PORT_1);

         if (eyeball.getLightValue() > 0 && eyeball.getLightValue() < 300) {
             state = 11;
         }

         if (eyeball.getLightValue() > 350 && eyeball.getLightValue() < 475) {
             state = 13;
         }

        if (eyeball.getLightValue() > 480 && eyeball.getLightValue() < 1000) {
            state = 12;
        }
    }

    public void do_state_14() {
            log("I'm in state 14. Searching for the target.");

            runMotor(PORT_A, 85); runMotor(PORT_B, 85);
            wait(50);
            stopMotor(PORT_A); stopMotor(PORT_B);

            eyeball = getActiveLightSensor(PORT_1);

            if (eyeball.getLightValue() > 350 && eyeball.getLightValue() < 475) {
                state = 11;
            }

            if (eyeball.getLightValue() > 480 && eyeball.getLightValue() < 1000) {
                state = 12;
            }
    }
}
